#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# Authors: kevin

import os
import math
import time
import numpy as np
from kyle_robot_toolbox.robot_arm.arm4dof_uservo import Arm4DoFUServo

if __name__ == '__main__':
    dir_path = '/home/rikibot/catkin_ws/src/rikibot_project/rikibot_grasp_race/config/'
    arm = Arm4DoFUServo(config_folder=dir_path, is_init_pose=False)
    arm.gripper_open()
    arm.set_tool_pose(pose_name="capture_image")
